/**
 * \file       :	MS5611.c
 * \brief      :	This document provides procedures for reading barometer MS5611 barometric data, height data.
 * \author     :	Huangj
 * \version    :	V1.0
 */
	/********************************************************************************
	 * Tips
	 * Use function ms5611_init() to initialized MS5611.
	 * Use function	ms5611_barometric_height() to get barometric height values.
	*********************************************************************************/
#include "ms5611.h"
#include "i2c.h"
#include "usart.h"
#include <math.h>

#define FAST_LOOP_PERIOD_MS          5
#define NUMBER_OF_ATMOSPHERIC        32               /* the number of atmospheric pressure value */ 

static I2C_HandleTypeDef *i2c_handle = &hi2c1;

static ms5611_dat_t g_ms5611_dat;
ms5611_dat_t *gp_ms5611_dat = &g_ms5611_dat;

/* The following two arrays are one-to-one values of atmospheric pressure and altitude */
float atmospheric_pressure_value[NUMBER_OF_ATMOSPHERIC] = {0,1.46,11.2,17.6,28.0,44.9,72.4,91.7,116.5,148.2,188.2,239.3,301.3,376.5,466.1,571.6,
                                                            696.4,724.0,752.2,781.9,812.2,843.3,859.1,874.9,891.5,908.1,924.6,941.9,959.1,976.3,994.9,1013.3};

                                        
float metre[NUMBER_OF_ATMOSPHERIC]                      = {0,45720,30510,27459,24408,21357,18306,16781,15255,13730,12204,10679,9153,7628,6102,
                                                            4577,3050,2746,2441,2136,1831,1526,1373,1220,1068,915,763,610,458,305,153,0};  

                                        
                                            
                                         

float y2a[NUMBER_OF_ATMOSPHERIC] = {0};


static uint16_t g_prom[8]         = { 0 };  /*!< This array is used to store PROM data */
static uint8_t  g_ms5611_data[6]  = { 0 };  /*!< This array is used to store conversion data */
static uint16_t g_ms5611_count    = 0;      /*!< This variable is used for state machines */
static uint32_t g_pressure        = 0;      /*!< Digital pressure value */
static uint32_t g_temperature     = 0;      /*!< Digital temperature value */
static float    g_dT              = 0;      /*!< Difference between actual and reference temperature */
static float    g_temp            = 0;      /*!< Actual temperature */
static float    g_press           = 0;      /*!< Temperature compensated pressure */
static float    g_height          = 0;      /*!< barometric height value */




/**
* \brief          This function is use to reset the Sensor
*
* \retval         void
*/
void ms5611_reset(void)
{
  uint32_t ms5611_timeout = MS5611_TIMEOUT_MAX; /*!< I2C read , write timeout */
  uint8_t  ms5611_reset_data[1] = { 0 };
 /********************************************************************************
  * A few clock pulses are required before sending a reset command to the sensor.
  * so send the command before the 0x00 to simulate the clock pulse
  *********************************************************************************/
  ms5611_reset_data[0] = MS5611_RESET;
//  bsp_i2c_tx_dma(I2C_1, MS5611_ADDRESS_WRITE, ms5611_reset_data, 1);
  HAL_I2C_Master_Transmit(i2c_handle, MS5611_ADDRESS_WRITE, ms5611_reset_data, 1, ms5611_timeout);
}
/**
 * \brief          This function is used to read the PROM data of the MS5611
 * \detail         6 coefficients necessary to compensate for process variations 
 *                 and temperature variations are calculated and stored in the 
 *                 128-bit PROM of each module.
 *
 * \retval         0xff : The reading process went wrong
 *                 0x00 : Read success
 */
uint8_t ms5611_read_prom(void)
{
    HAL_StatusTypeDef ms5611_status;
    uint32_t ms5611_timeout = MS5611_TIMEOUT_MAX; /*!< I2C read , write timeout */
    uint8_t  d1,d2          = 0;
    uint8_t  prom_data[2] 	= { 0 }; 
    
    for(uint8_t i=0; i<8; i++) {
    
        prom_data[0] = (MS5611_PROM_READ + (i * 2));
        

        
        ms5611_status = HAL_I2C_Master_Transmit (i2c_handle, MS5611_ADDRESS_WRITE, prom_data, 1, ms5611_timeout);
        if ( ms5611_status == HAL_OK) {
            ;
        } else {
     //       DE_PRINTF("HAL_I2C_Master_Transmit error! i = %d", i);
            while(i2c_handle->State != HAL_I2C_STATE_READY);
        }
        
        HAL_Delay(10);
        
        ms5611_status = HAL_I2C_Master_Receive (i2c_handle, MS5611_ADDRESS_READ, prom_data, 2, ms5611_timeout);
        if (ms5611_status == HAL_OK ) {
            d1 = prom_data[0];
            d2 = prom_data[1];
        } else {
     //       DE_PRINTF("HAL_I2C_Master_Receive error! i = %d", i);
            while(i2c_handle->State != HAL_I2C_STATE_READY);
        }
        g_prom[i] = ((uint16_t)d1 << 8) | d2;
    }
    
    
    return 0;
}
/**
 * \brief          Get temperature from Barometric Pressure Sensor
 *
 * \param[in,out]  void
 *
 * \retval         void
 */
void ms5611_calculate_temperature(void)
{
    g_dT          = g_temperature *1.0 - (((uint32_t)g_prom[5]) * 256.0);
    g_temp        = 2000 + ((g_dT*(uint32_t)g_prom[6])/8388608);
}
/**
 * \brief          Get temperature from Barometric Pressure Sensor
 *
 * \param[in,out]  void
 *
 * \retval         void
 */
void ms5611_calculate_pressure(void)
{
  double  off         = 0;    /*!< Offset at actual temperature */
  double  sens        = 0;    /*!< Sensitivity at actual temperature */
  double  temp2       = 0;    /*!< compensation temperature value */
  double  temp_square = 0;    /*!< compensation square of the actual temperature value */
  double  off2        = 0;    /*!< compensation offset at actual temperature */
  double  sens2       = 0;    /*!< compensation sensitivity at actual temperature */
   
    
  off   = (uint32_t)g_prom[2]*65536+((uint32_t)g_prom[4]*g_dT)/128;
  sens  = (uint32_t)g_prom[1]*32768+(((uint32_t)g_prom[3]*g_dT)/256);
    

    
 /********************************************************************************
  * Secondary temperature compensation.
  *********************************************************************************/
  if(g_temp<2000) {
    
    temp2       = (g_dT*g_dT) /0x80000000;
    temp_square = (g_temp - 2000) * (g_temp - 2000);
    off2        = 2.5f*temp_square;
    sens2       = 1.25f*temp_square;
      
    if(g_temp < -1500) {
        
        off2  = off2 + 7 * (g_temp + 1500) * (g_temp + 1500);
        sens2 = sens + 11 * (g_temp + 1500) * (g_temp + 1500) / 2;
        
    }           

  }
  
    g_temp      = g_temp - temp2;
    off         = off  - off2;
    sens        = sens - sens2;	
  
 /********************************************************************************
  * data analysis
  *********************************************************************************/
  g_temp        /=   100.f;
  g_press       =   (g_pressure * sens / 2097152.f - off) / 32768.f;
  //g_height      =   ((1 - pow((g_press / 101325), (1 / 5.25588))) / 0.0000225577);
  splint(atmospheric_pressure_value, metre, y2a, 31, g_press/100.0f, &g_height);

//  DE_PRINTF("temp = %f degree Celsius\r\n pressure = %f Pa\r\n height = %f m\r\n", g_temp, g_press, g_height);
}
/**
 * \brief          :	Initialize MS5611
 *
 * \retval         :	none
 */
void ms5611_init(void)
{
    ms5611_reset();
    
    HAL_Delay(500);
    
    ms5611_read_prom();

    spline(atmospheric_pressure_value, metre, 31, -1561.6016, -8.3152, y2a);
    
    
}
/**
 * \brief          : This function is used to read the sensor's barometric height value
 * \detail         : Applies to time polling to read data. --> Attention : FAST_LOOP_PERIOD_MS should be 5
 *
 * \param[out]     : height:   barometer height
 * \param[out]  	 : pressure: barometer pressure
 *
 * \retval         : barometric height values uint is mile
 */
void ms5611_get_data(int32_t *height, uint32_t *pressure)
{
    uint32_t ms5611_timeout = MS5611_TIMEOUT_MAX; /*!< I2C read , write timeout */
    uint32_t conversion;
    uint32_t conver1,conver2,conver3;


    uint8_t data[1] = { 0 };
    
    g_ms5611_count++;
    /********************************************************************************
     * Send the command to convert the temperature data.
     * Attention that the MS5611 takes 7.4-9.04ms to complete the conversion.
    *********************************************************************************/
    if(g_ms5611_count == 1) {
    
        data[0] = MS5611_D2_OSR_4096;
        HAL_I2C_Master_Transmit(i2c_handle, MS5611_ADDRESS_WRITE, data, 1, ms5611_timeout);
    
    }	else if (g_ms5611_count == 1 + (10 / FAST_LOOP_PERIOD_MS)) {
    /********************************************************************************
     * Wait 7.4-9.04ms for MS5611 to convert the temperature data.
     * Send command to read temperature data.
    *********************************************************************************/
        data[0] = 0x00;
        HAL_I2C_Master_Transmit(i2c_handle, MS5611_ADDRESS_WRITE, data, 1, ms5611_timeout);
    /********************************************************************************
     * Receive temperature data.
     *********************************************************************************/
        HAL_I2C_Master_Receive(i2c_handle, MS5611_ADDRESS_READ, g_ms5611_data, 3, ms5611_timeout);
    
    } else if (g_ms5611_count == 3 + (10 / FAST_LOOP_PERIOD_MS)) {
    /********************************************************************************
     * Analytical temperature data After receiving.
     * Send the command to convert the temperature data.
     * Attention that the MS5611 takes 7.4-9.04ms to complete the conversion.
     *********************************************************************************/
        conver1 = g_ms5611_data[0];
        conver2 = g_ms5611_data[1];
        conver3 = g_ms5611_data[2];
        conversion = (conver1 << 16) | (conver2 << 8) | conver3;
        g_temperature = conversion;
        
        ms5611_calculate_temperature();

        data[0] = MS5611_D1_OSR_4096;
        HAL_I2C_Master_Transmit(i2c_handle, MS5611_ADDRESS_WRITE,	data, 1, ms5611_timeout);
    
    } else if(g_ms5611_count == 3 + 2 * (10 / FAST_LOOP_PERIOD_MS)) {
    /********************************************************************************
     * Wait 7.4-9.04ms for MS5611 to convert the pressure data.
     * Send command to read pressure data.
     *********************************************************************************/
        data[0] = 0x00;
        HAL_I2C_Master_Transmit(i2c_handle, MS5611_ADDRESS_WRITE, data, 1, ms5611_timeout);

    /********************************************************************************
     * Receive pressure data.
    *********************************************************************************/
        HAL_I2C_Master_Receive(i2c_handle, MS5611_ADDRESS_READ, g_ms5611_data, 3, ms5611_timeout);
    
    } else if	(g_ms5611_count == 5 + 2 * (10 / FAST_LOOP_PERIOD_MS)) {
    /********************************************************************************
     * Analytical temperature data After receiving.
    *********************************************************************************/
        conver1 = g_ms5611_data[0];
        conver2 = g_ms5611_data[1];
        conver3 = g_ms5611_data[2];
        conversion = (conver1 << 16) | (conver2 << 8) | conver3;
        g_pressure = conversion;
        

        ms5611_calculate_pressure();
    
    } else if (g_ms5611_count >= (1000 / FAST_LOOP_PERIOD_MS + 1)) {
    /********************************************************************************
     * Reading barometric data every 200 ms.
    *********************************************************************************/
        g_ms5611_count = 0;
    }
    
    *height   = (int32_t)g_height;
    *pressure = (uint32_t)g_press;
}
/**
 * \brief          : Task of geting barometric data from ms5611
 *
 * \param[in]      : ctx :context
 * \param[in]      : word:word should be printf
 *
 * \retval         : void
 */
//void ms5611_func(task_t *s, void* ctx)
//{
//    int32_t baro_height = 0;
//    uint32_t pressure    = 0;
//    ms5611_get_data(&baro_height, &pressure);
//    //DE_PRINTF("barometer height = %d m, pressure = %d Pa", baro_height, pressure);
//}	
/********************************************************************************
 * Example
 * 01. Reset.
 * 02. Read PROM data
 * 03. Send command to convert the temperature data.
 * 04. Wait at least 9.04 ms to complete the convertion. Then 
 * 05. 
 * 06.
 * 07.
 * 08.
 * 09.
*********************************************************************************/
/**
  * @brief  Master Rx Transfer completed callback.
  * @param  hi2c Pointer to a I2C_HandleTypeDef structure that contains
  *                the configuration information for the specified I2C.
  * @retval None
  */
void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
	if(hi2c->Instance == I2C1) {
		
	}
	;
}
/** @brief  Slave Tx Transfer completed callback.
  * @param  hi2c Pointer to a I2C_HandleTypeDef structure that contains
  *                the configuration information for the specified I2C.
  * @retval None
  */
void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c)
{
	;
}


void spline(float x[], float y[], int n, float yp1, float ypn, float y2[])
{
	int i,k;
	float p,qn,sig,un,*u;

	u=vector(1,n-1);
	if (yp1 > 0.99e30f)
		y2[1]=u[1]=0.0f;
	else {
		y2[1] = -0.5f;
		u[1]=(3.0f/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
	}
	for (i=2;i<=n-1;i++) {
		sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		p=sig*y2[i-1]+2.0f;
		y2[i]=(sig-1.0f)/p;
		u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i]=(6.0f*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}
	if (ypn > 0.99e30f)
		qn=un=0.0;
	else {
		qn=0.5;
		un=(3.0f/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
	}
	y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+1.0f);
	for (k=n-1;k>=1;k--)
		y2[k]=y2[k]*y2[k+1]+u[k];
	free_vector(u,1,n-1);
}

float *vector(long nl, long nh)
/* allocate a float vector with subscript range v[nl..nh] */
{
	float *v;

	v=(float *)malloc((size_t) ((nh-nl+1+1)*sizeof(float)));
//	if (!v) nrerror("allocation failure in vector()");
	return v-nl+1;
}


void free_vector(float *v, long nl, long nh)
/* free a float vector allocated with vector() */
{
	free((char*) (v+nl-1));
}



void splint(float xa[], float ya[], float y2a[], int n, float x, float *y)
{
//	void nrerror(char error_text[]);
	int klo,khi,k;
	float h,b,a;

	klo=1;
	khi=n;
	while (khi-klo > 1) {
		k=(khi+klo) >> 1;
		if (xa[k] > x) khi=k;
		else klo=k;
	}
	h=xa[khi]-xa[klo];
//	if (h == 0.0) nrerror("Bad xa input to routine splint");
	a=(xa[khi]-x)/h;
	b=(x-xa[klo])/h;
    
    float c = a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0f;
	*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0f;
    
}



